home *** CD-ROM | disk | FTP | other *** search
/ Amiga Format CD 46 / Amiga Format CD46 (1999-10-20)(Future Publishing)(GB)[!][issue 1999-12].iso / -in_the_mag- / reader_requests / scilab / demos / flat / car.sci next >
Text File  |  1999-09-16  |  15KB  |  556 lines
  1. function []=mvcr(x,y,theta,phi)
  2. ///////////////%% BEGIN OF SCRIPT-FILE mvcr %%%%%%%%%%%%%%%
  3. xbasc() ;
  4. //
  5. //  CAR PACKING VIA FLATNESS AND FRENET FORMULAS
  6. //
  7. //    explicit computation and visualisation of the motions.
  8. //
  9. //    February 1993
  10. //
  11. // ............................................................
  12. // :         pierre ROUCHON  <rouchon@cas.ensmp.fr>           :
  13. // : Centre Automatique et Systemes, Ecole des Mines de Paris :
  14. // : 60, bd Saint Michel -- 75272 PARIS CEDEX 06, France      :
  15. // :    Telephone: (1) 40 51 91 15 --- Fax: (1) 43 54 18 93   :
  16. // :..........................................................:
  17. //
  18. //
  19. // bigL:  car length (m) 
  20. // bigT: basic time interval for one  smooth motion (s)
  21. // a0, a1, p(3): intermediate variables for polynomial 
  22. //           curves definition
  23. //
  24. //
  25. bigT = 1 ; bigL = 1 ;
  26. a0 =0 ; a1 = 0 ;
  27. p= [0 0 0 ] ;
  28. //
  29. // initial configuration of the car
  30. x1 = x ; y1 = y ; theta1 = theta ; phi1 = phi ;
  31. // final configuration of the car 
  32. x2 = 0 ; y2 = 0  ; theta2 =0; phi2 = 0 ;
  33. // Constraints: y1 > y2 and -%pi/2 < theta1,2, phi1,2 < %pi/2
  34. //
  35. //  sampling of motion 1 --> 0 and 0 --> 2 
  36.      nbpt = 40 ;
  37. // computation of  intermediate configuration 
  38. x0 = maxi(x1,x2)   ....
  39.     + bigL*abs(tan(theta1)) .....
  40.     + bigL*abs(tan(theta2)) .....
  41.     + bigL*(abs(y1-y2)/bigL)^(1/2) ;
  42. y0 = (y1+y2)/2 ;
  43. //
  44. //
  45. // first polynomial curve
  46. a0 = x0 ; b0 = y0 ;
  47. a1 = x1 ; b1 = y1 ; 
  48. M = [ 
  49.   (a1-a0)^3     (a1-a0)^4     (a1-a0)^5 
  50. 3*(a1-a0)^2   4*(a1-a0)^3   5*(a1-a0)^4
  51. 6*(a1-a0)    12*(a1-a0)^2  20*(a1-a0)^3 
  52.       ] ;
  53. q = [ 
  54.    b1-b0 
  55.    tan(theta1) 
  56.    tan(phi1)/(bigL*(cos(theta1)^3)) 
  57.     ] ;
  58. p = inv(M)*q ;
  59. //
  60. // computation the first motion, time: 1 -> 0
  61.  state=[ x1 y1 theta1 phi1 ] ;
  62.  for i=1:(nbpt+1)
  63.     tau = (i-1)/nbpt ;
  64.     phi=tau*tau*(3-2*tau) ;
  65.     a = (1-phi)*a1 + phi*a0 ;
  66.     f=  b0+ p(1).*(a-a0)^3 +    p(2).*(a-a0)^4 +    p(3).*(a-a0)^5 ;
  67.    df  = 3*p(1).*(a-a0)^2 +  4*p(2).*(a-a0)^3 +  5*p(3).*(a-a0)^4 ;
  68.    ddf = 6*p(1).*(a-a0)   + 12*p(2).*(a-a0)^2 + 20*p(3).*(a-a0)^3 ;
  69.    k = ddf / ((1+df*df)^(3/2)) ;
  70.    state = [ state; a   f  atan(df) atan(k*bigL)] ; 
  71.  end  
  72. //
  73. //
  74. // second polynomial curve
  75. a0 = x0 ; b0 = y0 ;
  76. a1 = x2 ; b1 = y2 ; 
  77. M = [ 
  78.   (a1-a0)^3     (a1-a0)^4     (a1-a0)^5 
  79. 3*(a1-a0)^2   4*(a1-a0)^3   5*(a1-a0)^4
  80. 6*(a1-a0)    12*(a1-a0)^2  20*(a1-a0)^3 
  81.       ] ;
  82. q = [ 
  83.       b1-b0 
  84.    tan(theta2) 
  85.    tan(phi2)/(bigL*(cos(theta2)^3)) 
  86.     ] ;
  87. p = inv(M)*q ;
  88. //
  89. // computation of the second motion 0 --> 2
  90.  for i=1:(nbpt+1)
  91.    tau = (i-1)/nbpt ;
  92.    phi=tau*tau*(3-2*tau) ;
  93.    a = (1-phi)*a0 + phi*a1 ;
  94.    f=  b0+ p(1).*(a-a0)^3 +    p(2).*(a-a0)^4 +    p(3).*(a-a0)^5 ;
  95.    df  = 3*p(1).*(a-a0)^2 +  4*p(2).*(a-a0)^3 +  5*p(3).*(a-a0)^4 ;
  96.    ddf = 6*p(1).*(a-a0)   + 12*p(2).*(a-a0)^2 + 20*p(3).*(a-a0)^3 ;
  97.    k = ddf / ((1+df*df)^(3/2)) ;
  98.    state = [ state; a   f  atan(df) atan(k*bigL)] ;
  99.  end  
  100. //
  101. //
  102. // Graphics 
  103. //
  104. // window size
  105. xmini = mini(state(:,1))-0.5*bigL ;
  106. xmaxi = maxi(state(:,1))+1.5*bigL ;
  107. ymini = mini(state(:,2))-1.5*bigL ;
  108. ymaxi = maxi(state(:,2))+1.5*bigL ;
  109.  
  110. //xsetech([0,0,1,1],[xmini,ymini,xmaxi,ymaxi]);
  111. isoview(xmini,xmaxi,ymini,ymaxi)
  112.  
  113. // starting configuration
  114. ptcr([x1,y1,theta1,phi1]) ;
  115. // end configuration
  116. ptcr([x2,y2,theta2,phi2]) ;
  117. // intermediate configuration (inversion of velocity)
  118. ptcr([x0,y0,0,0]) ;
  119. // trajectory of the linearizing output
  120. xpoly(state(:,1),state(:,2),'lines') ;
  121. // movies 
  122. [n m] = size(state) ;
  123. xset('alufunction',6);...
  124. for i=1:n,
  125.   ptcr( state(i,:)) ; ptcr( state(i,:)) ;
  126. end ;
  127. xset('alufunction',3);...
  128. // animated version with the pixmap driver 
  129. // pixb=xget("pixmap");
  130. // xset("pixmap",1)
  131. // for i=1:n,xset("wwpc");
  132. //    ptcr([x1,y1,theta1,phi1]) ;
  133. //    ptcr([x2,y2,theta2,phi2]) ;
  134. //    ptcr([x0,y0,0,0]) ;
  135. //    xpoly(state(:,1),state(:,2),'lines') ;
  136. //     ptcr( state(i,:)) ;
  137. //    xset("wshow");    
  138. //      end ;
  139. //xset("pixmap",pixb);
  140.  
  141. //%%%%%%%% END OF SCRIPT-FILE mvcr %%%%%%%%%%%%%%%
  142.  
  143. function []=mvcr2T(x,y,theta1,theta2,theta3,phi)
  144. xbasc();
  145. //
  146. //  CAR WITH 2 TRAILERS PACKING VIA FLATNESS AND FRENET FORMULAS
  147. //
  148. //   explicit computation and visualisation of the motions.
  149. //
  150. //    February 1993
  151. //
  152. // ............................................................
  153. // :         pierre ROUCHON  <rouchon@cas.ensmp.fr>           :
  154. // : Centre Automatique et Systemes, Ecole des Mines de Paris :
  155. // : 60, bd Saint Michel -- 75272 PARIS CEDEX 06, France      :
  156. // :    Telephone: (1) 40 51 91 15 --- Fax: (1) 43 54 18 93   :
  157. // :..........................................................:
  158. //
  159. // lengths 
  160. //    bigL:  car length (m) 
  161. //   d1: trailer 1 length (m)
  162. //   d2: trailer 2 length (m)
  163. // bigT: basic time interval for one  smooth motion (s)
  164. // a0, a1, b0, p(5): intermediate variables for polynomial 
  165. //           curves definition
  166. //
  167. bigT = 1 ; 
  168. bigL = 1 ; d1 = 1.5 ; d2 = 1 ; 
  169. a0 =0 ; a1 = 0 ; b0 = 0 ;
  170. p= [0 0 0 0 0 ] ;
  171. //
  172. // initial configuration
  173. //   the system is described via the coordinates of last trailer
  174. //    
  175. x2_1 = x ; y2_1 = y ; 
  176. theta2_1= theta1; theta12_1 = theta2 ; theta01_1= theta3 ;
  177. phi_1 = phi ;
  178. //
  179. // final configuration of the car 
  180. x2_2 = 0 ; y2_2 = 0  ;
  181. theta2_2= 0 ; theta12_2 = 0 ; theta01_2= 0 ;
  182.                                    phi_2 = 0 ;
  183. //
  184. // sampling of motion 1 --> 0 and of motion 0 --> 2
  185.         nbpt1 = 40 ; nbpt2 = 40 ;
  186. //
  187. // Constraints: y2_1 > y2_2 and 
  188. // the 4 angles  theta2_1,2
  189. //               theta12_1,2
  190. //               theta01_1,2
  191. //               phi_1,2 
  192. //   must belong to  ] -%pi/2 , + %pi/2 [
  193. //               
  194. //
  195. // conputation of  intermediate configuration
  196. LL=bigL+d1+d2
  197. x2_0 = maxi(x2_1,x2_2) ....
  198.       + LL*abs(tan(theta2_1)) ....
  199.       + LL*abs(tan(theta12_1)) .... 
  200.       + LL*abs(tan(theta01_1)) .... 
  201.       + LL*( abs(y2_1-y2_2)/(d1+d2+bigL) )^(1/2) ;
  202. y2_0 = (y2_1+y2_2)/2 ;
  203. //
  204. //
  205. // first polynomial curve
  206. a0 = x2_0 ; b0 = y2_0 ;
  207. a1 = x2_1 ; b1 = y2_1 ; 
  208. p=cr2Tkf((b1-b0),theta2_1,theta12_1,theta01_1,phi_1) ;
  209. //
  210. // computation the first motion  1 -> 0
  211.  theta2 = theta2_1 ;
  212.  theta1 = theta12_1+theta2 ;
  213.  theta0 = theta01_1+theta1 ;
  214.  phi = phi_1 ;
  215.  x0=x2_1+d2*cos(theta2)+d1*cos(theta1) ;
  216.  y0=y2_1+d2*sin(theta2)+d1*sin(theta1) ;
  217.  state_1 = [x0 y0 theta0 theta1 theta2 phi] ;
  218.  for i=1:(nbpt1+1)
  219.    tau = (i-1)/nbpt1 ;
  220.    phi=tau*tau*(3-2*tau) ;
  221.    aa = (1-phi)*a1 + phi*a0 ;
  222.    [bb df d2f d3f d4f d5f] = cr2Tfjt(aa) ;
  223.    [k2 k1 k0 dk0]=cr2Tfk(df,d2f,d3f,d4f,d5f) ;
  224.    theta2 = atan(df);
  225.    theta1 = atan(k2*d2)+theta2;
  226.    theta0 = atan(k1*d1) + theta1 ;
  227.    phi = atan(k0*bigL) ;
  228.    x0=aa+d2*cos(theta2)+d1*cos(theta1) ;
  229.    y0=bb+d2*sin(theta2)+d1*sin(theta1) ;
  230.    state_1 = [ state_1 ; x0 y0 theta0 theta1 theta2 phi] ;
  231.  end ;
  232. //
  233. // second polynomial curve
  234. a0 = x2_0 ; b0 = y2_0 ;
  235. a1 = x2_2 ; b1 = y2_2 ; 
  236.  p=cr2Tkf((b1-b0),theta2_2,theta12_2,theta01_2,phi_2) ;
  237. //
  238. // computation of the second motion  0 -> 2
  239.  theta2 = 0 ;
  240.  theta1 = 0 ;
  241.  theta0 = 0 ;
  242.  phi = 0 ;
  243.  x0=x2_0+d2*cos(theta2)+d1*cos(theta1) ;
  244.  y0=y2_0+d2*sin(theta2)+d1*sin(theta1) ;
  245.  state_2 = [x0 y0 theta0 theta1 theta2 phi] ;
  246.  for i=1:(nbpt2+1)
  247.    tau = (i-1)/nbpt2 ;
  248.    phi=tau*tau*(3-2*tau) ;
  249.    aa = (1-phi)*a0 + phi*a1 ;
  250.    [bb df d2f d3f d4f d5f] = cr2Tfjt(aa) ;
  251.    [k2 k1 k0 dk0]=cr2Tfk(df,d2f,d3f,d4f,d5f) ;
  252.    theta2 = atan(df);
  253.    theta1 = atan(k2*d2)+theta2;
  254.    theta0 = atan(k1*d1) + theta1 ;
  255.    phi = atan(k0*bigL) ;
  256.    x0=aa+d2*cos(theta2)+d1*cos(theta1) ;
  257.    y0=bb+d2*sin(theta2)+d1*sin(theta1) ;
  258.    state_2 = [ state_2 ; x0 y0 theta0 theta1 theta2 phi] ;
  259.  end ;
  260. //
  261. // Graphics 
  262. //
  263. // window size
  264. xmini = mini([mini(state_1(:,1)) mini(state_2(:,1))]) -1.5*(d1+d2)  ;
  265. xmaxi = maxi([maxi(state_1(:,1)) maxi(state_1(:,1))]) +1.5*bigL ;
  266. ymini = mini([mini(state_1(:,2)) mini(state_2(:,2))])-bigL;
  267. ymaxi = maxi([maxi(state_1(:,2)) maxi(state_1(:,2))])+bigL;
  268. xsetech([0,0,1,1],[xmini,ymini,xmaxi,ymaxi]);
  269. isoview(xmini,xmaxi,ymini,ymaxi)
  270. //
  271.  
  272. xy_T1 = [ [-bigL/3  bigL/3   bigL/3  -bigL/3  -bigL/3  
  273. -bigL/3 -bigL/3 bigL/3  bigL/3  -bigL/3 ], .....
  274.  [ bigL/3  d1;
  275.   0     0], .....
  276.  [-bigL/8   bigL/8 
  277.  bigL/6  bigL/6 
  278.  ], .... 
  279.  [-bigL/8   bigL/8 
  280. -bigL/6 -bigL/6  ] ] ;
  281.  
  282. xy_T2 = [[-bigL/3  bigL/3   bigL/3  -bigL/3  -bigL/3  
  283. -bigL/3 -bigL/3 bigL/3  bigL/3  -bigL/3 ],...
  284.  [bigL/3  d2
  285.   0         0 ],[ -bigL/8   bigL/8 
  286.  bigL/6  bigL/6 
  287.  ],[ -bigL/8   bigL/8 
  288. -bigL/6 -bigL/6 ] ] ;
  289.  
  290. // starting configuration
  291.    x2=x2_1 ; y2=y2_1 ;
  292.    theta2 = theta2_1 ;
  293.    theta1 = theta12_1+theta2 ;
  294.    theta0 = theta01_1+theta1 ;
  295.    phi = phi_1 ;
  296.    x1=x2+d2*cos(theta2) ;
  297.    y1=y2+d2*sin(theta2) ;
  298.    x0=x1+d1*cos(theta1) ;
  299.    y0=y1+d1*sin(theta1) ;
  300. ptcr2T([x0,y0,theta0,theta1,theta2,phi]) ;
  301. // end configuration
  302.    x2=x2_2 ; y2=y2_2 ;
  303.    theta2 = theta2_2 ;
  304.    theta1 = theta12_2+theta2 ;
  305.    theta0 = theta01_2+theta1 ;
  306.    phi = phi_2 ;
  307.    x1=x2+d2*cos(theta2) ;
  308.    y1=y2+d2*sin(theta2) ;
  309.    x0=x1+d1*cos(theta1) ;
  310.    y0=y1+d1*sin(theta1) ;
  311. ptcr2T([x0,y0,theta0,theta1,theta2,phi]) ;
  312. // intermediate configuration (inversion of velocity)
  313.    x2=x2_0 ; y2=y2_0 ;
  314.    theta2 = 0 ;
  315.    theta1 = 0;
  316.    theta0 = 0 ;
  317.    phi = 0;
  318.    x1=x2+d2*cos(theta2) ;
  319.    y1=y2+d2*sin(theta2) ;
  320.    x0=x1+d1*cos(theta1) ;
  321.    y0=y1+d1*sin(theta1) ;
  322. ptcr2T([x0,y0,theta0,theta1,theta2,phi]) ;
  323. state_1 =[state_1;state_2] ;
  324. x_lin = state_1(:,1)-d1*cos(state_1(:,4))-d2*cos(state_1(:,5)) ;
  325. y_lin = state_1(:,2)-d1*sin(state_1(:,4))-d2*sin(state_1(:,5)) ;
  326. //  motion
  327. //
  328. // trajectory of the linearizing output
  329. xpoly(x_lin,y_lin,'lines')
  330. // movies 
  331. [n m] = size(state_1) ;
  332. xset('alufunction',6);
  333. for j=1:n
  334.   ptcr2T(state_1(j,:));ptcr2T(state_1(j,:));
  335. end ;
  336. xset('alufunction',3);
  337.  
  338. ////%%%%%%%%%%%% END OF SCRIPT-FILE mvcr2T %%%%%%%%%%%%%
  339.  
  340. function []=dbcr()
  341. //
  342. //  CAR PACKING VIA FLATNESS AND FRENET FORMULAS
  343. //
  344. //   debugg and verification via integration 
  345. //        of the non holonomic system
  346. //
  347. //                   February 1993
  348. //
  349. // ............................................................
  350. // :         pierre ROUCHON  <rouchon@cas.ensmp.fr>           :
  351. // : Centre Automatique et Systemes, Ecole des Mines de Paris :
  352. // : 60, bd Saint Michel -- 75272 PARIS CEDEX 06, France      :
  353. // :    Telephone: (1) 40 51 91 15 --- Fax: (1) 43 54 18 93   :
  354. // :..........................................................:
  355. //
  356. //
  357. // bigL:  car length (m) 
  358. // bigT: basic time interval for one  smooth motion (s)
  359. // a0, a1, p(3): intermediate variables for polynomial 
  360. //           curves definition
  361. //
  362. //
  363. bigT = 1 ; bigL = 1 ;
  364. a0 =0 ; a1 = 0 ;
  365. p= [0 0 0 ] ;
  366. //
  367. // initial configuration of the car
  368. x1 = 0 ; y1 = 4 ; theta1 = %pi/2.5 ; phi1 = 0 ;
  369. // final configuration of the car 
  370. x2 = 0 ; y2 = 0  ; theta2 =0; phi2 = 0 ;
  371. // Constraints: y1 > y2 and -%pi/2 < theta1,2, phi1,2 < %pi/2
  372. //
  373. // conputation of  intermediate configuration 
  374. x0 = maxi(x1,x2)   ....
  375.     + bigL*abs(tan(theta1)) .....
  376.     + bigL*abs(tan(theta2)) .....
  377.     + bigL*(abs(y1-y2)/bigL)^(1/2) ;
  378. y0 = (y1+y2)/2 ;
  379. //
  380. //
  381. // first polynomial curve
  382. a0 = x0 ; b0 = y0 ;
  383. a1 = x1 ; b1 = y1 ; 
  384. M = [ 
  385.   (a1-a0)^3     (a1-a0)^4     (a1-a0)^5 
  386. 3*(a1-a0)^2   4*(a1-a0)^3   5*(a1-a0)^4
  387. 6*(a1-a0)    12*(a1-a0)^2  20*(a1-a0)^3 
  388.       ] ;
  389. q = [ 
  390.    b1-b0 
  391.    tan(theta1) 
  392.    tan(phi1)/(bigL*(cos(theta1)^3)) 
  393.     ] ;
  394. p = inv(M)*q ;
  395. //
  396. // simulation of the first motion, time: 0 -> bigT
  397.  [t_1,state_1]=ode23('car',0,bigT, [ x1 y1 theta1 phi1 ]);
  398. //
  399. //
  400. // second polynomial curve
  401. a0 = x0 ; b0 = y0 ;
  402. a1 = x2 ; b1 = y2 ; 
  403. M = [ 
  404.   (a1-a0)^3     (a1-a0)^4     (a1-a0)^5 
  405. 3*(a1-a0)^2   4*(a1-a0)^3   5*(a1-a0)^4
  406. 6*(a1-a0)    12*(a1-a0)^2  20*(a1-a0)^3 
  407.       ] ;
  408. q = [ 
  409.       b1-b0 
  410.    tan(theta2) 
  411.    tan(phi2)/(bigL*(cos(theta2)^3)) 
  412.     ] ;
  413. p = inv(M)*q ;
  414. //
  415. // simulation of the second motion, time: bigT -> 2bigT
  416.  [n m]=size(t_1);
  417.  [t_2,state_2]=ode23('car',bigT,2*bigT, state_1(n,:) );
  418. //
  419. //
  420. // result array merging
  421.  t_1=t_1(2:n) ; state_1=state_1(2:n,:);
  422.  t=[
  423.     t_1 
  424.     t_2
  425.     ]; 
  426. state = [
  427.          state_1 
  428.          state_2 
  429.         ];
  430. //
  431. //
  432.  plot(t,state) ;
  433. // xlabel('time (s)') ; 
  434. // ylabel('x y theta phi ') ;
  435. //%%%%%%%%%//%%%% END OF SCRIPT-FILE dbcr  %%%%%%%%%%%%
  436.  
  437.  
  438. function []=dbcr2T()
  439. //
  440. //  CAR WITH 2 TRAILERS PACKING VIA FLATNESS AND FRENET FORMULAS
  441. //  
  442. //  debugg and verification via the integration 
  443. //        of the non holonomic system.
  444. //
  445. //    February 1993
  446. //
  447. // ............................................................
  448. // :         pierre ROUCHON  <rouchon@cas.ensmp.fr>           :
  449. // : Centre Automatique et Systemes, Ecole des Mines de Paris :
  450. // : 60, bd Saint Michel -- 75272 PARIS CEDEX 06, France      :
  451. // :    Telephone: (1) 40 51 91 15 --- Fax: (1) 43 54 18 93   :
  452. // :..........................................................:
  453. //
  454. // lengths 
  455. //    bigL:  car length (m) 
  456. //   d1: trailer 1 length (m)
  457. //   d2: trailer 2 length (m)
  458. // bigT: basic time interval for one  smooth motion (s)
  459. // a0, a1, p(5): intermediate variables for polynomial 
  460. //           curves definition
  461. //
  462. bigT = 1 ; 
  463. bigL = 1 ; d1 = 1.5 ; d2 = 1 ; 
  464. a0 =0 ; a1 = 0 ; b0 = 0 ;
  465. p= [0 0 0 0 0 ] ;
  466. //
  467. // initial configuration
  468. //   the system is described via the coordinates of last trailer
  469. x2_1 = 0 ; y2_1 = 6 ; 
  470. theta2_1= %pi/8; theta12_1 =  %pi/8 ; theta01_1= %pi/8 ;
  471.                                   phi_1 = %pi/8 ;
  472. //
  473. // final configuration of the car 
  474. x2_2 = 0 ; y2_2 = 0  ;
  475. theta2_2= 0 ; theta12_2 = 0 ; theta01_2= 0 ;
  476.                                    phi_2 = 0 ;
  477. //
  478. // Constraints: y2_1 > y2_2 and 
  479. // the 4 angles  theta2_1,2
  480. //               theta12_1,2
  481. //               theta01_1,2
  482. //               phi_1,2 
  483. //   must belong to  ] -%pi/2 , + %pi/2 [
  484. //               
  485. //
  486. // conputation of  intermediate configuration
  487. LL=bigL+d1+d2 ;
  488. x2_0 = maxi(x2_1,x2_2) ....
  489.       + LL*abs(tan(theta2_1)) ....
  490.       + LL*abs(tan(theta12_1)) .... 
  491.       + LL*abs(tan(theta01_1)) .... 
  492.       + LL*( abs(y2_1-y2_2)/(d1+d2+bigL) )^(1/2) ;
  493. y2_0 = (y2_1+y2_2)/2 ;
  494. //
  495. //
  496. //
  497. // first polynomial curve
  498. a0 = x2_0 ; b0 = y2_0 ;
  499. a1 = x2_1 ; b1 = y2_1 ; 
  500. p=cr2Tkf((b1-b0),theta2_1,theta12_1,theta01_1,phi_1) ;
  501. //
  502. // simulation of the first motion  0 -> T
  503. //   time t between 0 and bigT
  504.  theta2 = theta2_1 ;
  505.  theta1 = theta12_1+theta2 ;
  506.  theta0 = theta01_1+theta1 ;
  507.  phi = phi_1 ;
  508.  x0=x2_1+d2*cos(theta2)+d1*cos(theta1) ;
  509.  y0=y2_1+d2*sin(theta2)+d1*sin(theta1) ;
  510.  [t_1,state_1]=ode45('car2T',0,bigT, ....
  511.                      [ x0 y0 theta0 theta1 theta2 phi ]);
  512. // graphics 
  513.  subplot(121);
  514.    plot(t_1,state_1(:,1:2)) ;
  515.    xlabel('time (s)') ; 
  516.    ylabel('x_car y_car (m)') ;
  517.  subplot(122);
  518.    plot(t_1,state_1(:,3:6)) ;
  519.    xlabel('time (s)') ; 
  520.    ylabel('theta0 theta1 theta2 phi (rd)') ;
  521. //
  522. //
  523. // second polynomial curve
  524. a0 = x2_0 ; b0 = y2_0 ;
  525. a1 = x2_2 ; b1 = y2_2 ; 
  526.  p=cr2Tkf((b1-b0),theta2_2,theta12_2,theta01_2,phi_2) ;
  527. //
  528. // simulation of the second motion  bigT -> 2*bigT
  529. //
  530. // important remark: due to numerical instability of the  
  531. //  integration during inverse motion, we integrate
  532. //  from the final position 2 to the intermediate position 0.
  533. //
  534.  theta2 = theta2_2 ;
  535.  theta1 = theta12_2+theta2 ;
  536.  theta0 = theta01_2+theta1 ;
  537.  phi = phi_2 ;
  538.  x0=x2_2+d2*cos(theta2)+d1*cos(theta1) ;
  539.  y0=y2_2+d2*sin(theta2)+d1*sin(theta1) ;
  540.  [t_2,state_2]=ode45('car2T',0,bigT, ....
  541.                      [ x0 y0 theta0 theta1 theta2 phi ]);
  542. //
  543. //
  544. //
  545. // graphics 
  546.   t_2 = 2*bigT - t_2 ;
  547.  subplot(121);
  548.    plot(t_2,state_2(:,1:2)) ;
  549.    xlabel('time (s)') ; 
  550.    ylabel('x_car y_car (m)') ;
  551.  subplot(122);
  552.    plot(t_2,state_2(:,3:6)) ;
  553.    xlabel('time (s)') ; 
  554.    ylabel('theta0 theta1 theta2 phi (rd)') ;
  555. //%%%%%%%%%%%%%% END OF SCRIPT-FILE dbcr2T  %%%%%%%%%%%%
  556.